Systems and methods for fully coupled models for crowd navigation

ABSTRACT

Systems and methods for utilizing interactive Gaussian processes for crowd navigation are provided. In one embodiment, a system for a crowd navigation includes a processor, a statistical module, and a model module. The processor receives sensor data. The statistical module identifies a number of agents in a physical environment based on the sensor data. The statistical module further calculates a set of Gaussian processes. The set of Gaussian processes includes a Gaussian Process for each agent of the number of agents. The statistical module further determines an objective function based on an intent and a flexibility for the host and at least two agent of the plurality of agents. The model module generates a model of the number of agents by applying the objective function to the set of Gaussian processes. The model includes a convex configuration of the number of agents in the physical environment.

CROSS-REFERENCE TO RELATED APPLICATION

This application is related to U.S. Provisional Application Ser. No.62/937,334 filed on Nov. 19, 2019, which is expressly incorporatedherein by reference. Additionally, this application is related to U.S.Provisional Application Ser. No. 62/799,481 filed on Jan. 31, 2019,which is expressly incorporated herein by reference. Furthermore, theapplication is related to U.S. Provisional Application Ser. No.62/899,676 filed on Sep. 12, 2019, which is expressly incorporatedherein by reference. This application is also related to U.S.Non-provisional application Ser. No. 16/743,777 filed on Jan. 15, 2020,which is expressly incorporated herein by reference.

BACKGROUND

Constructing realistic, real time, human-robot interaction models is acore challenge in crowd navigation. Typically, work has focused onrobot-agent coupling, referred to as a first order interaction, whileignoring agent-agent coupling, referred to as a second orderinteraction. In 2010, a freezing robot problem (FRP) was discovered whenindependently modeling a host for crowd navigation. In particular, thehost navigating a group of humans caused the host to freeze (or takeunnecessary evasive maneuvers that negatively impacted efficiency) ascongestion worsened. The FRP was demonstrated experimentally in a sixmonth university cafeteria study by showing that the host-agentindependence assumption caused a 3× decrement in complete stopping fordensities above 0.55 people/m². Further, the FRP had been reproduced inmultiple studies indicating its impact on crowd navigation. Althoughmodeling of the host in a crowd has been attempted, very little is knownabout what form a collision avoidance function should take. Further,merely formulating the collision avoidance function is insufficientbecause inference is non-trivial.

BRIEF DESCRIPTION

According to one embodiment, a system for a crowd navigation of a hostvehicle among a plurality of agents is provided. The system includes aprocessor, a statistical module, and a model module. The processorreceives sensor data. The statistical module identifies a number ofagents in a physical environment based on the sensor data. Thestatistical module further calculates a set of Gaussian processes. Theset of Gaussian processes includes a Gaussian Process for each agent ofthe number of agents. The statistical module further determines anobjective function based on an intent and a flexibility for the host andat least two agent of the plurality of agents. The model modulegenerates a model of the number of agents by applying the objectivefunction to the set of Gaussian processes. The model includes a convexconfiguration of the number of agents in the physical environment.

According to another embodiment, a method for crowd navigation of a hostvehicle among a plurality of agents is provided. The method includesidentifying a number of agents in a physical environment based on thesensor data. The method also includes calculating a set of Gaussianprocesses. The set of Gaussian processes includes a Gaussian Process foreach agent of the number of agents. The method further includesdetermining an objective function based on an intent and a flexibilityfor the host and at least two agent of the plurality of agents. Themethod yet further includes generating a model of the number of agentsby applying the objective function to the set of Gaussian processes. Themodel includes a convex configuration of the number of agents in thephysical environment.

According to yet another embodiment, a non-transitory computer readablestorage medium storing instructions that, when executed by a computerhaving a processor, cause the computer to perform a method for crowdnavigation of a host vehicle among a plurality of agents. The methodincludes identifying a number of agents in a physical environment basedon the sensor data. The method also includes calculating a set ofGaussian processes. The set of Gaussian processes includes a GaussianProcess for each agent of the number of agents. The method furtherincludes determining an objective function based on an intent and aflexibility for the host and at least two agent of the plurality ofagents. The method yet further includes generating a model of the numberof agents by applying the objective function to the set of Gaussianprocesses. The model includes a convex configuration.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is an exemplary component diagram of a system for crowdnavigation, according to one aspect.

FIG. 2 is an exemplary process flow of a method for crowd navigation,according to one aspect.

FIG. 3A is another exemplary agent environment for a system for crowdnavigation, according to one aspect.

FIG. 3B is another exemplary agent environment for a system for crowdnavigation, according to one aspect.

FIG. 3C is another exemplary agent environment for a system for crowdnavigation, according to one aspect.

FIG. 4 is an illustration of an example computer-readable medium orcomputer-readable device including processor-executable instructionsconfigured to embody one or more of the provisions set forth herein,according to one aspect.

FIG. 5 is an illustration of an example computing environment where oneor more of the provisions set forth herein are implemented, according toone aspect.

DETAILED DESCRIPTION

Systems and methods for crowd navigation utilizing fully coupled modelsof first order interactions and second order interactions to mitigatethe FRP and unnecessary evasive maneuvers that negatively impact safetyand efficiency. Considering both the first order interactions and thesecond order interactions substantially improves safety, efficiency, andperformance.

In particular, the systems and methods here in provide an inference on astatistically valid joint over Gaussian process mixture (GPM) agentmodels by first formulating the joint using a generic interactionfunction and then leverage the GP mixtures to constrain the jointcollision avoidance function.

The means are interpreted as optimization variables which are modulatedby the GP covariances. Effectively, then, we are optimizing over afunction space—rather than a state space of time orderedpositions—because the covariance encodes a smooth evolution oftrajectories as they mutually shape each other. Further, by usinginitialization functions, the real time approach is competitive withexhaustive search of the non-convex solution space.

A system and method for crowd navigation utilizing interacting Gaussianmixture models such as zero free-parameter Gaussian processes (zpIGP)for crowd navigation in congested environments is disclosed. Inparticular, the agent models may be Gaussian mixture models (GMM). Forexample, dynamical models with Gaussian noise are GMMs; deep networkswith LSTM outputs. Here, a joint collision avoidance function composedof GMM/GPM agent models is provided, thereby providing insight into theoptimality structure of this important class of crowd navigationdistributions. Moreover, the joint collision avoidance function providesan efficiency statistical profile competitive with humans thatoutperforms the other crowd navigation systems and methods.

Definitions

The following includes definitions of selected terms employed herein.The definitions include various examples and/or forms of components thatfall within the scope of a term and that can be used for implementation.The examples are not intended to be limiting. Furthermore, thecomponents discussed herein, can be combined, omitted, or organized withother components or into different architectures.

“Bus,” as used herein, refers to an interconnected architecture that isoperably connected to other computer components inside a computer orbetween computers. The bus can transfer data between the computercomponents. The bus can be a memory bus, a memory processor, aperipheral bus, an external bus, a crossbar switch, and/or a local bus,among others. The bus can also be a vehicle bus that interconnectscomponents inside a vehicle using protocols such as Media OrientedSystems Transport (MOST), Controller Area network (CAN), LocalInterconnect network (LIN), among others.

“Component,” as used herein, refers to a computer-related entity (e.g.,hardware, firmware, instructions in execution, combinations thereof).Computer components may include, for example, a process running on aprocessor, a processor, an object, an executable, a thread of execution,and a computer. A computer component(s) can reside within a processand/or thread. A computer component can be localized on one computerand/or can be distributed between multiple computers.

“Computer communication,” as used herein, refers to a communicationbetween two or more communicating devices (e.g., computer, personaldigital assistant, cellular telephone, network device, vehicle, vehiclecomputing device, infrastructure device, roadside equipment) and can be,for example, a network transfer, a data transfer, a file transfer, anapplet transfer, an email, a hypertext transfer protocol (HTTP)transfer, and so on. A computer communication can occur across any typeof wired or wireless system and/or network having any type ofconfiguration, for example, a local area network (LAN), a personal areanetwork (PAN), a wireless personal area network (WPAN), a wirelessnetwork (WAN), a wide area network (WAN), a metropolitan area network(MAN), a virtual private network (VPN), a cellular network, a token ringnetwork, a point-to-point network, an ad hoc network, a mobile ad hocnetwork, a vehicular ad hoc network (VANET), a vehicle-to-vehicle (V2V)network, a vehicle-to-everything (V2X) network, avehicle-to-infrastructure (V2I) network, among others. Computercommunication can utilize any type of wired, wireless, or networkcommunication protocol including, but not limited to, Ethernet (e.g.,IEEE 802.3), WiFi (e.g., IEEE 802.11), communications access for landmobiles (CALM), WiMax, Bluetooth, Zigbee, ultra-wideband (UWAB),multiple-input and multiple-output (MIMO), telecommunications and/orcellular network communication (e.g., SMS, MMS, 3G, 4G, LTE, 5G, GSM,CDMA, WAVE), satellite, dedicated short range communication (DSRC),among others.

“Communication interface” as used herein can include input and/or outputdevices for receiving input and/or devices for outputting data. Theinput and/or output can be for controlling different vehicle features,which include various vehicle components, systems, and subsystems.Specifically, the term “input device” includes, but is not limited to:keyboard, microphones, pointing and selection devices, cameras, imagingdevices, video cards, displays, push buttons, rotary knobs, and thelike. The term “input device” additionally includes graphical inputcontrols that take place within a user interface which can be displayedby various types of mechanisms such as software and hardware-basedcontrols, interfaces, touch screens, touch pads or plug and playdevices. An “output device” includes, but is not limited to, displaydevices, and other devices for outputting information and functions.

“Computer-readable medium,” as used herein, refers to a non-transitorymedium that stores instructions and/or data. A computer-readable mediumcan take forms, including, but not limited to, non-volatile media, andvolatile media. Non-volatile media can include, for example, opticaldisks, magnetic disks, and so on. Volatile media can include, forexample, semiconductor memories, dynamic memory, and so on. Common formsof a computer-readable medium can include, but are not limited to, afloppy disk, a flexible disk, a hard disk, a magnetic tape, othermagnetic medium, an ASIC, a CD, other optical medium, a RAM, a ROM, amemory chip or card, a memory stick, and other media from which acomputer, a processor or other electronic device can read.

“Database,” as used herein, is used to refer to a table. In otherexamples, “database” can be used to refer to a set of tables. In stillother examples, “database” can refer to a set of data stores and methodsfor accessing and/or manipulating those data stores. In one embodiment,a database can be stored, for example, at a disk, data store, and/or amemory. A database may be stored locally or remotely and accessed via anetwork.

“Data store,” as used herein can be, for example, a magnetic disk drive,a solid-state disk drive, a floppy disk drive, a tape drive, a Zipdrive, a flash memory card, and/or a memory stick. Furthermore, the diskcan be a CD-ROM (compact disk ROM), a CD recordable drive (CD-R drive),a CD rewritable drive (CD-RW drive), and/or a digital video ROM drive(DVD ROM). The disk can store an operating system that controls orallocates resources of a computing device.

“Display,” as used herein can include, but is not limited to, LEDdisplay panels, LCD display panels, CRT display, touch screen displays,among others, that often display information. The display can receiveinput (e.g., touch input, keyboard input, input from various other inputdevices, etc.) from a user. The display can be accessible throughvarious devices, for example, though a remote system. The display mayalso be physically located on a portable device, mobility device, orhost.

“Logic circuitry,” as used herein, includes, but is not limited to,hardware, firmware, a non-transitory computer readable medium thatstores instructions, instructions in execution on a machine, and/or tocause (e.g., execute) an action(s) from another logic circuitry, module,method and/or system. Logic circuitry can include and/or be a part of aprocessor controlled by an algorithm, a discrete logic (e.g., ASIC), ananalog circuit, a digital circuit, a programmed logic device, a memorydevice containing instructions, and so on. Logic can include one or moregates, combinations of gates, or other circuit components. Wheremultiple logics are described, it can be possible to incorporate themultiple logics into one physical logic. Similarly, where a single logicis described, it can be possible to distribute that single logic betweenmultiple physical logics.

“Memory,” as used herein can include volatile memory and/or nonvolatilememory. Non-volatile memory can include, for example, ROM (read onlymemory), PROM (programmable read only memory), EPROM (erasable PROM),and EEPROM (electrically erasable PROM). Volatile memory can include,for example, RAM (random access memory), synchronous RAM (SRAM), dynamicRAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDRSDRAM),and direct RAM bus RAM (DRRAM). The memory can store an operating systemthat controls or allocates resources of a computing device.

“Module,” as used herein, includes, but is not limited to,non-transitory computer readable medium that stores instructions,instructions in execution on a machine, hardware, firmware, software inexecution on a machine, and/or combinations of each to perform afunction(s) or an action(s), and/or to cause a function or action fromanother module, method, and/or system. A module can also include logic,a software-controlled microprocessor, a discrete logic circuit, ananalog circuit, a digital circuit, a programmed logic device, a memorydevice containing executing instructions, logic gates, a combination ofgates, and/or other circuit components. Multiple modules can be combinedinto one module and single modules can be distributed among multiplemodules.

“Operable connection,” or a connection by which entities are “operablyconnected,” is one in which signals, physical communications, and/orlogical communications can be sent and/or received. An operableconnection can include a wireless interface, firmware interface, aphysical interface, a data interface, and/or an electrical interface.

“Portable device,” as used herein, is a computing device typicallyhaving a display screen with user input (e.g., touch, keyboard) and aprocessor for computing. Portable devices include, but are not limitedto, handheld devices, mobile devices, smart phones, laptops, tablets,e-readers, smart speakers. In some embodiments, a “portable device”could refer to a remote device that includes a processor for computingand/or a communication interface for receiving and transmitting dataremotely.

“Processor,” as used herein, processes signals and performs generalcomputing and arithmetic functions. Signals processed by the processorcan include digital signals, data signals, computer instructions,processor instructions, messages, a bit, a bit stream, that can bereceived, transmitted and/or detected. Generally, the processor can be avariety of various processors including multiple single and multicoreprocessors and co-processors and other multiple single and multicoreprocessor and co-processor architectures. The processor can includelogic circuitry to execute actions and/or algorithms.

“Vehicle,” as used herein, refers to any moving vehicle that is capableof carrying one or more users and is powered by any form of energy. Theterm “vehicle” includes, but is not limited to cars, trucks, vans,minivans, SUVs, motorcycles, scooters, boats, go-karts, amusement ridecars, rail transport, personal watercraft, and aircraft. In some cases,a motor vehicle includes one or more engines. Further, the term“vehicle” can refer to an electric vehicle (EV) that is capable ofcarrying one or more users and is powered entirely or partially by oneor more electric motors powered by an electric battery. The EV caninclude battery electric vehicles (BEV) and plug-in hybrid electricvehicles (PHEV). The term “vehicle” can also refer to an autonomousvehicle and/or self-driving vehicle powered by any form of energy. Theautonomous vehicle can carry one or more users. Further, the term“vehicle” can include vehicles that are automated or non-automated withpre-determined paths or free-moving vehicles.

“Vehicle system,” as used herein can include, but is not limited to, anyautomatic or manual systems that can be used to enhance the vehicle,driving, and/or safety. Exemplary vehicle systems include, but are notlimited to: an electronic stability control system, an anti-lock brakesystem, a brake assist system, an automatic brake prefill system, a lowspeed follow system, a cruise control system, a collision warningsystem, a collision mitigation braking system, an auto cruise controlsystem, a lane departure warning system, a blind spot indicator system,a lane keep assist system, a navigation system, a steering system, atransmission system, brake pedal systems, an electronic power steeringsystem, visual devices (e.g., camera systems, proximity sensor systems),a climate control system, an electronic pretensioning system, amonitoring system, a passenger detection system, a vehicle suspensionsystem, a vehicle seat configuration system, a vehicle cabin lightingsystem, an audio system, a sensory system, an interior or exteriorcamera system among others.

I. System Overview

Referring now to the drawings, the drawings are for purposes ofillustrating one or more exemplary embodiments and not for purposes oflimiting the same. FIG. 1 is an exemplary component diagram of anoperating environment 100 for utilizing fully coupled Gaussian mixturemodels for crowd navigation, according to one aspect. The operatingenvironment 100 includes a sensor module 102, a computing device 104,and operational systems 106 interconnected by a bus 108. The componentsof the operating environment 100, as well as the components of othersystems, hardware architectures, and software architectures discussedherein, may be combined, omitted, or organized into differentarchitectures for various embodiments. The computing device 104 may beimplemented with a device or remotely stored.

Considering a vehicle embodiment, the computing device 104 may beimplemented as part of a telematics unit, a head unit, a navigationunit, an infotainment unit, an electronic control unit, among others ofa host, such as the host 302 shown in FIG. 3 . The host 302 may be avehicle, robot, or other self-propelled machine. In other embodiments,the components and functions of the computing device 104 can beimplemented, for example, with other devices 530 (e.g., a portabledevice) or another device connected via a network (e.g., a network 132).The computing device 104 may be capable of providing wired or wirelesscomputer communications utilizing various protocols to send/receiveelectronic signals internally to/from components of the operatingenvironment 100. Additionally, the computing device 104 may be operablyconnected for internal computer communication via the bus 108 (e.g., aController Area Network (CAN) or a Local Interconnect Network (LIN)protocol bus) to facilitate data input and output between the computingdevice 104 and the components of the operating environment 100.

The computing device 104 includes a processor 112, a memory 114, a datastore 116, and a communication interface 118, which are each operablyconnected for computer communication via a bus 108 and/or other wiredand wireless technologies. The communication interface 118 providessoftware and hardware to facilitate data input and output between thecomponents of the computing device 104 and other components, networks,and data sources, which will be described herein. Additionally, thecomputing device 104 also includes a statistical module 120 and a modelmodule 122, for crowd navigation facilitated by the components of theoperating environment 100.

The statistical module 120 may be an artificial neural network that actsas a framework for machine learning, including deep learning. The modelmodule 122 may be a decoder that converts the data generated by thestatistical module 120 to a model of the physical environments shown inFIGS. 3A, 3B, and 3C. The predicted labels of the model module 122 maybe labels that correspond to future actions based on the sensor data110. Continuing the vehicular example given above, the label maycorrespond to a predicted maneuver of the host 302. In some embodiments,the predicted maneuver may include a series of maneuvers (e.g.,going-straight, right-turn, left-turn, decelerate, etc.). In someembodiments, the labels may be directed to maneuvers of the vehicle.Additionally, or alternatively the labels may be learned or receivedfrom a remote server (not shown).

The computing device 104 is also operably connected for computercommunication (e.g., via the bus 108 and/or the communications interface118) to one or more operational systems 106. The operational systems 106can include, but are not limited to, any automatic or manual systemsthat can be used to enhance the device, operation, and/or safety. Theoperational systems 106 may dependent on the implementation. Forexample, given a vehicular embodiment, the operational systems 106include a brake system 124, a path planning module 126, a notificationsystem 128, and a sensor system 130 according to an exemplaryembodiment. The brake system 124 monitors, analyses, and calculatesbraking information and facilitates features like anti-lock brakesystem, a brake assist system, and an automatic brake prefill system.The path planning module 126 monitors, analyses, operates the device tosome degree. For example, the path planning module 126 may store,calculate, and provide directional information and facilitates featureslike vectoring and obstacle avoidance among others. The notificationsystem 128 identifies notifications, generates notifications, andfacilitates communication.

The operational systems 106 also include and/or are operably connectedfor computer communication to the sensor system 130. The sensor system130 provides and/or senses information associated with a device (e.g.,the host 302), the operating environment 100, an environment of thedevice, and/or the operational systems 106. The sensor system 130 caninclude, but is not limited to, environmental sensors, vehicle speedsensors, accelerator pedal sensors, brake sensors, wheel sensors, amongothers. In some embodiments, the sensor system 130 is incorporated withthe operational systems 106. For example, one or more sensors of thesensor system 130 may be incorporated with the brake system 124 monitorcharacteristics of the host 302, such as deceleration.

Accordingly, the sensor system 130 is operable to sense a measurement ofdata associated with the device, the operating environment 100, thedevice environment, and/or the operational systems 106 and generate adata signal indicating said measurement of data. These data signals canbe converted into other data formats (e.g., numerical) and/or used bythe sensor module 102, the computing device 104, and/or the operationalsystems 106 to generate other data metrics and parameters. It isunderstood that the sensors can be any type of sensor, for example,acoustic, electric, environmental, optical, imaging, light, pressure,force, thermal, temperature, proximity, among others.

The sensor module 102, the computing device 104, and/or the operationalsystems 106 are also operatively connected for computer communication tothe network 132. The network 132 is, for example, a data network, theInternet, a wide area network (WAN) or a local area (LAN) network. Thenetwork 132 serves as a communication medium to various remote devices(e.g., databases, web servers, remote servers, application servers,intermediary servers, client machines, other portable devices). Usingthe system and network configuration discussed above, anomalous eventscan be detected. Detailed embodiments describing exemplary methods usingthe system and network configuration discussed above will now bediscussed in detail.

II. Methods for Crowd Navigation

Referring now to FIG. 2 , a method 200 for crowd navigation will now bedescribed according to an exemplary embodiment. FIG. 2 will also bedescribed with reference to FIGS. 1 and 3A-5 . For simplicity, themethod 200 will be described as a sequence of elements, but it isunderstood that the elements of the method 200 can be organized intodifferent architectures, blocks, stages, and/or processes.

At block 202 the method 200 includes the statistical module 120identifying a number of agents. Turning to FIG. 3A, the agents 304-312are entities moving in a physical environment of the host 302. Dependingon the movement, path planning, and/or capability identify the positionof the agents 304-312. The agents 304-312 may be biological entities(humans, animals, insects), vehicles, robots, etc. The agents 304-312may be identified based on sensor data 110 including, visual data,motion data, and physiological data, among others. In this manner, thestatistical module 120 may detect or identify one or more of theentities, objects, obstacles, hazards, and/or corresponding attributesor characteristics including agent identification, a position or alocation associated with the agents 304-312, such as a lane location,coordinates, position, size of the agents 304-312, trajectory, velocity,acceleration, etc.

The statistical module 120 may also model the characteristics andattributes of the agents 304-312 relative to the host 302, as shown inFIG. 3B. The characteristics and attributes determined by thestatistical module 120 of agents 304-312 relative to the host 302 may beclassified as first order interactions. The first order interactions mayinclude, for example, an agent identification, how the trajectories ofthe agents 304-312 coincide with the path planning of the host 302,speeds of the agents 304-312 relative to the host 302, distances of theagents 304-312 from the host 302, a bearing or direction of travel ofthe agents 304-312 relative to the host 302, acceleration of the agents304-312 relative to the host 302, etc. Moreover, the statistical module120 may determine if one or more of the agents 304-312 is attempting tocommunicate with the host. Interplay between the host 302 and one ormore of the agents 304-312 may be determined based on eyelid movement,head movement and/or mouth movement of the agents 304-312. Thestatistical module 120 may measure the degree of head movement such asthe tilt of the head of the agents 304-312 relative to the host 302 orthe angle of the head of the agents 304-312 relative to the host 302.

In some embodiments, the host 302 may measure the relative gap betweenagents 308 and 310 relative to the host 302 as well as the relative gapbetween agents 310 and 312 relative to the host 302. The statisticalmodule 120 may determine that the gap between the agents 308 and 310 islarger and thus better able to accommodate the host 302, as shown inFIG. 3B.

Furthermore, the statistical module 120 may determine attributes orcharacteristics of the agents 304-312 relative to one another, as shownin FIG. 3C. The characteristics and attributes determined by thestatistical module 120 of agents 304-312 relative to one another may beclassified as second order interactions. The first order interactionsmay include, for example, position or a location of the agents 304-312relative to one another, speeds of the agents 304-312 relative to oneanother, distances of the agents 304-312 relative to one another, abearing or direction of travel of the agents 304-312 relative to oneanother, acceleration of the agents 304-312 relative to one another,distances of the agents 304-312 relative to one another, a, such as alane location, coordinates, etc. The statistical module 120 maydetermine if the agents 304-312 are communicating with one another.Interplay between one or more of the agents 304-312 may be determinedbased on eyelid movement, head movement and/or mouth movement of theagents 304-312. The statistical module 120 may measure the degree ofhead movement such as the tilt of the head of the agents 304-312relative to other agents or the angle of the head of the agents 304-312relative to other agents. For example, the statistical module 120 mayidentify the interplay of the agents 310 and 312 based on the gapbetween the agents 310 and 312, corresponding mouth movements betweenthe agents 310 and 312.

In one or more embodiments, the statistical module 120 may identifywhite lines and hard shoulders of a roadway or road segment tofacilitate lane recognition. In another embodiment, the statisticalmodule 120 may identify infrastructure of the physical environment.Further, the statistical module 120 may identify or classify an agent ofthe agents 304-312 as different types of agents, for example, apedestrian, vehicle, a cyclist, etc. The different types may be based onthe speed at which the agent moves, the size of the agent, and/or othersensor data 110.

The sensor module 102 receives sensor data 110. The sensor data 110 maybe received from the sensor system 130, remote devices (e.g., via thebus 108 and/or the communications interface 118), and/or a biologicalentity. The sensor data 110 may include a video sequence or a series ofimages, user inputs, and/or data from the operational systems 106, suchas data from a Controller Area Network (CAN) bus including as pedalpressure, steer angle, etc. The sensor system 130 may include one ormore radar units, image capture components, sensors, cameras,gyroscopes, accelerometers, scanners (e.g., 2-D scanners or 3-Dscanners), or other measurement components. In some embodiments, thesensor data 110 is augmented as additional sensor data from othersources is received. For example, the data from the CAN bus may beaugmented by information the agents 304-312, the types of agent, andimage/video data, among others.

At block 204 the method 200 includes calculating a Gaussian Process foreach agent of the agents 304-312 and the host 302. A Gaussian Process isa stochastic process (a collection of random variables indexed by timeor space), such that every finite collection of those random variableshas a multivariate normal distribution, i.e. every finite linearcombination of them is normally distributed. Here the Gaussian Processesmay be defined as X to be the state of the agents 304-312 and the host302. For example, X could be

² for planar navigation. The measurements z_(1:t) ^(R) of the trajectoryof the host 302 f^(R):t∈

→X and n_(t) measurements z_(1:t) ¹, . . . , z_(1:t) ^(n) ^(t) of thehuman trajectories f=[f¹,f^(n) ^(t) ]:t∈

→X, where f represents the crowd. The functions f^(R) and f may begoverned by p(f^(R)|z_(1:t) ^(R)) and p(f^(i)|z_(1:t) ^(f) ^(i) ) foreach i. Herein, the shorthand z_(1:t) ^(f)=[z¹:t^(f) ¹ , . . . , z_(1:t)^(f) ^(n) ^(t)] may be used. Further, fR, f^(i)∈

(X), the function space over X and the shorthand

(x|μ, Σ)≡GP(x|μ, Σ) may be used by the statistical module 120 to definethe Gaussian processes of the agents 304-312 and the host 302 as:

$\begin{matrix}{{{p\left( {f^{i}{❘z_{1:t}^{f^{i}}}} \right)} = {\overset{N_{t}^{f^{i}}}{\underset{k_{i} = 1}{\Sigma}}w_{k_{i}}^{f^{i}}\left( {f^{i}{❘{\mu_{k_{i}}^{f^{i}},\overset{f^{i}}{\underset{k_{i}}{\Sigma}}}}} \right)}}{{{for}{each}{of}{the}{agents}304 - 312},{and}}{{p\left( {f^{R}{❘z_{1:t}^{R}}} \right)} = {\underset{l = 1}{\sum\limits^{N_{t}^{R}}}{w_{\ell}^{R}\left( {f^{R}{❘{\mu_{\ell}^{R},\overset{R}{\underset{\ell}{\Sigma}}}}} \right){for}{the}{host}302.}}}} & \left( {{Eq}.01} \right)\end{matrix}$

Furthermore, as the statistical module 120 may continually calculate theGaussian processes as sensor data 110 is received. For clarity, weexpress time:

≡

(t), . . . ,

w_(k_(n_(t)))^(f^(n_(t))) ≡ w_(k_(n_(t)))^(f^(n_(t)))(t)and μ≡μ(t), Σ≡Σ(t). In this manner, the Gaussian processes may bedefined as follows:

Definition 1: Let N_(f) _(R) _(,l)=N(f^(R)|

,

). For each f^(i), let N_(f,v)p=N(f^(p)|μ_(v) ^(p), Σ_(v) ^(p)) wherev=k_(i) and p∈{1, . . . , n_(t)}. For example p(f^(R)|z_(1:t) ^(R))=

.

Definition 2: The intents of the agents 304-312 and the host 302 aregiven by μ_(k) _(i) ^(f) ^(i) ,

respectively. If N_(t) ^(f) ^(i) or N_(t) ^(R) is greater than 1, intentambiguity is present. Intent preferences are w_(k) ^(f) ^(i) and

.

Definition 3: Flexibility is the willingness of the agents 304-312 tocompromise their intent. Mathematically, the flexibility of intent μ isΣ.

In Equation 0.1, intent preferences are the data likelihood, e.g. w_(k)_(R) ^(R)=

(f^(R)=z_(1:t) ^(R)μ_(k) _(R) ^(R), Σ_(k) _(R) ^(R)). To generate theGaussian processes the data z_(1:t) ^(R), z_(1:t) ^(f) is used. Letp(f^(i)|z_(1:t) ^(f) ^(i) )=Σ_(k) _(i) _(=L,R)w_(k) _(i) ^(f) ^(i)

_(f) _(i) _(,k) _(i) . Because N_(t) ^(f) ^(i) =2, intent ambiguity ispresent over left and right intents. Flexibility is motivated by thefollowing. Draw an agent sample x and evaluate according to

(f^(i)=x|μ, Σ). For large covariance, large deviation from intentreturns nontrivial probabilities (e.g., large flexibility); for smallcovariance, large deviation from intent returns vanishing probability.

In this manner, the crowd navigation uses a joint host-crowd densityp(f^(R), f|z_(1:t) ^(R), z_(1:t) ^(f)) to generate the actionu_(t)=f_(t+1) ^(R)* at time t according to:[f ^(R) , . . . ,f ^(n) ^(t) ]*=arg max p(f ^(R) ,ff ^(R) , . . . ,f^(n) ^(t) |z _(1:t) ^(R) ,z _(1:t) ^(f))  (Eq. 0.2)

At block 206 of the method 200, an objective function is determinedbased on the intent and flexibility. For example, suppose the intent ofthe host 302 is to avoid a collision with the agent 312 while alsocontinuing to move in as direct a path as possible. In this manner, theintent can be a balance between competing goals. The flexibility is thedegree to which the host 302 can deviate from the intent. In thismanner, the Gaussian processes define the intent and flexibility for thehost 302 and the agents 304-312 as the mean and variance, respectively,of the function ψ.

Starting with the Gaussian processes, a set of principles is derivedthat the objective function—the function coupling the agentmodels—conforms to. Accordingly, the objective function is determinedbased on desired safety and efficiency properties and an optimizationroutine to find μ_(t)=f_(t+1) ^(R)*.

$\begin{matrix}{{p\left( {f^{R},{f{❘{z_{1:t}^{R},z_{1:t}^{f}}}}} \right)} = {{{\psi\left( {f^{R},f,\gamma} \right)}{p\left( {f{❘z_{1:t}^{f}}} \right)}{p\left( {f^{R}{❘z_{1:t}^{R}}} \right)}} = {\prod\limits_{i = 1}^{n_{t}}{{\psi\left( {f^{R},f^{i},\gamma} \right)}{p\left( {f^{i}{❘z_{1:t}^{f^{i}}}} \right)}{p\left( {f^{R}{❘z_{1:t}^{R}}} \right)}}}}} & \left( {{Eq}.0.3} \right)\end{matrix}$

The function ψ(f^(R), f, γ), γ∈

is a product of pairwise objective functions ψ(f^(R), f^(i), γ)modulated by p(f^(i)|z_(1:t) ^(f) ^(i) ). We generalize the bi-agentinteraction case, for example, for a host 302 and two interactingagents, such as the agents 306 and 308, such as:p(f ^(R) ,f|z _(1:t))=ψ(f ^(R) ,f ¹,γ)ψ(f ^(R) ,f ²,γ)×p(f ^(R) |z_(1:t) ^(R))p(f ¹ |z _(1:t) ^(f) ¹ )p(f ² |z _(1:t) ^(f) ² ).

Now suppose that the hosts is interacting with three agents, such as theagents 306, 308, and 310, then the objective function is given by:ψ(f ^(R) ,f ¹,γ)ψ(f ^(R) ,f ²,γ)ψ(f ^(R) ,f ³,γ)×ψ(f ¹ ,f ²,γ)ψ(f ¹ ,f³,γ)ψ(f ² ,f ³,γ)

Generalized the objective function is given by:

${p\left( {f^{R},{f{❘{z_{1:t}^{R},z_{1:t}^{f}}}}} \right)} = {{{\psi\left( {f^{R},f,\gamma} \right)}{p\left( {f^{R}{❘z_{1:t}^{R}}} \right)}{p\left( {f{❘z_{1:t}^{f}}} \right)}} = {\prod\limits_{i = 1}^{n_{t}}{{\psi\left( {f^{R},f^{i},\gamma} \right)}{\prod\limits_{j > i}^{n_{t}}{{\psi\left( {f^{i},f^{j},\gamma} \right)}{p\left( {f^{R}{❘z_{1:t}^{R}}} \right)}{p\left( {f^{i}{❘z_{1:t}^{f^{i}}}} \right)}}}}}}$

To determine statistically valid forms of ψ(f^(i), f^(j), γ), wherei∈{R, 1, . . . , n_(t)}. Since p(f^(i)|z_(1:t) ^(i)), p(f^(j)|z_(1:t)^(j)) encode intention and flexibility information, the means andcovariances capture inter-agent intention and flexibility that isspecific to and influenced by agent interaction. If the interactionfunction has finite support (e.g.,

${\psi\left( {f^{i},f^{j},\gamma} \right)} = {\underset{t = 1}{\overset{T}{\Pi}}\left\lbrack {1 - {\exp\left( {{- \frac{1}{2\gamma}}\left( {f_{t}^{i} - f_{t}^{j}} \right)} \right)}} \right\rbrack}$where γ>0), joint flexibility is altered in a static and generic way;any interaction probability mass encoded in ψ(f^(i), f^(j), γ) altersthe agent-specific flexibilities (e.g., agent 1 and agent 2 are flexiblewith each other in a specific way, which is already captured in p(f^(i)z_(1:t) ^(f) ^(i) ), p(f^(j) z_(1:t) ^(f) ^(j) ). To preserve thestatistics of p(f^(i)|z_(1:t) ^(f) ^(i) ), p(f^(j)|z_(1,t) ^(f) ^(j) )we introduce the following transform, where f_(d) ^(i)˜p(f^(i)|z_(1:t)^(f) ^(t) ), f_(7n) ^(j)˜p(f^(j)|z_(1:t) ^(f) ^(j) ):δ(f^(R), f^(i))

Suppose

${{\overset{¯}{\delta}\left( {f_{d}^{i},f_{m}^{j}} \right)} \equiv {\lim\limits_{\gamma\rightarrow 0}\left\lbrack {\underset{\tau = 1}{\overset{T}{\Pi}}\left( {1 - {\exp\left\lbrack {{- \frac{1}{2\gamma}}\left( {{f_{d}^{i}(\tau)} - {f_{m}^{j}(\tau)}} \right)^{2}} \right\rbrack}} \right)} \right\rbrack}} = \left\{ {\begin{matrix}{{1\ {if}\ {\nexists{t \in {\left\lbrack {1,\ T} \right\rbrack\ {such}\ {that}\ {f_{d}^{i}(t)}}}}} = {f_{m}^{j}(t)}} \\{{0\ {if}\ {\exists{t \in {\left\lbrack {1,\ T} \right\rbrack\ {such}{\ }{that}\ {f_{d}^{i}(t)}}}}} = {f_{m}^{j}(t)}}\end{matrix},} \right.$

Given ψ(f^(i), x, y) has finite support over f^(i) then it may alteragent flexibility via Σ_(k) _(i) ^(f) ^(i) of

_(f) _(i) _(,k) _(i) . Let ψ(f^(i), f^(j), γ)=c_(k) _(i) _(,k) _(j) ;then using Equation 0.1 and 0.3, is given by

${p\left( {f^{i},{f^{j}{❘z_{1:t}}}} \right)} = {\overset{N_{t}^{f^{i}}}{\underset{k = 1}{\Sigma}}{\underset{k = 1}{\Sigma}}^{N_{t}^{f^{j}}}c_{k_{i},k_{j}}w_{k_{\lambda}}^{f^{i}}w_{k_{ϰ}}^{f^{j}}\mathcal{N}_{f^{i},k_{i}}{N_{f{jk}j}.}}$

Although the joint undergoes distortion from the effect of c_(k) _(i)_(,k) _(j) on the component weights w_(k) _(i) ^(f) ^(i) w_(k) _(j) ^(f)^(j) , no individual Σ_(k) _(i) ^(f) ^(i) or Σ_(k) _(j) ^(f) ^(j) incomponent

_(f) _(R) _(,l)

_(f) _(i) _(,k) _(i) is altered. Thus, the flexibility ofp(f^(i)|z_(1:t) ^(f) ^(i) ), p(f^(j)|z_(1:t) ^(f) ^(j) ) is preservedunder c_(k) _(i) _(,k) _(j) . Finally, let ψ(f^(R), f^(i), γ)=δ(f^(R),f^(i)). Let f_(a) ^(i)˜p(f^(i)|z_(1:t) ^(f) ^(i) ), f_(b)^(R)˜p(f^(R)|z_(1:t) ^(R)), η_(a) ^(f) ^(i) =p(f^(i)=f_(a) ^(i)|z_(1:t)^(f) ^(i) ), and η_(b) ^(R)=p(f^(R)=f_(b) ^(R)|z_(1:t) ^(R)). Then, forall b, p(f_(b) ^(i), f_(a) ^(j)|z_(1:t))=δ(f_(b) ^(i), f_(a) ^(j))η_(b)^(f) ^(i) η_(a) ^(f) ^(j) . If

t∈[1, T] such that f_(b) ^(i)(t)=f_(a) ^(j)(t), then δ(f_(b) ^(i), f_(a)^(j))η_(b) ^(f) ^(i) η_(a) ^(f) ^(j) =η_(b) ^(f) ^(i) η_(a) ^(f) ^(j) ;otherwise, it is zero. Thus, δ(f^(i), f^(j)) respects the agentflexibility data contained in p(f^(i)|z_(1:t) ^(f) ^(i) ). The sameargument can be made for p(f^(i)|z_(1:t) ^(f) ^(i) ); thus, δ(f^(i),f^(j)) respects the flexibility data in p(f^(i)|z_(1:t) ^(f) ^(i) ) andp(f^(j)|z_(1:t) ^(f) ^(j) ). Using a trajectory basis so that p(f^(i),f^(j) z_(1:t))=Σ_(g) ^(G)w_(g)δ([f^(i), f^(j)]−[f^(i), f^(j)]_(g)) thenδ(f^(i), f^(j)) would be applicable: discard intersecting samples. Sincethe interaction of two GPs is probabilistic, a “coupling” probability isappropriate.

At block 208 the method 200 includes the model module 122 generating amodel for the agents the 304-312 relative to the host. The model isgenerated by applying the objective function to the set of Gaussianprocesses. For Gaussian Processes, the probability of host-agentcollision does not only involve time-aligned terms: z_(k) _(i) _(,k)_(j) _(,t) ⁻¹=∫

(z|μ_(k) _(i) _(,t) ^(f) ^(t) , σ_(f) _(i) _(,k) _(i) ^(t,t))

(x|μ_(k) _(j) _(,t) ^(f) ^(j) , σ_(f) _(j) _(,k) _(j) ^(t,t)) whereμ_(k) _(i) _(,t) ^(f) ^(i) =μ_(k) _(i) ^(f) ^(i) (t), μ_(k) _(j) _(,t)^(f) ^(j) =μ_(k) _(j) ^(f) ^(j) (t)∈

² and i ∈{R, 1, . . . , n_(t)}; σ^(t,t) t is the t^(th) diagonal of Σ.Since Σ_(l) ^(R), Σ_(k) _(i) ^(f) ^(i) are dense, positions arecorrelated via covariance off diagonals and ∫

(x|μ_(k) _(i) _(,t) ^(f) ^(i) , σ_(f) _(i) _(,k) _(i) ^(t,τ))

(x|μ_(f) _(i) _(,k) _(i) ^(t,τ))=w_(k) _(i) _(,k) _(j)_(,t)×exp[−½(μ_(k) _(i) _(,t) ^(f) ^(i) −μ_(k,τ) ^(f) ^(j) )^(T)(σ_(k)_(i) _(+k) _(j) ^(t,τ)I)⁻¹(μ_(k) _(i) _(,t) ^(f) ^(λ) )]≡w_(k) _(i)_(,k) _(j) _(,t)Z_(k) _(i) _(,k) _(j) _(,t,τ) ⁻¹, contributes tocollision probability (σ_(k) _(i) _(+k) _(j) ^(t,τ) is the (t, τ)'thelement of Σ_(k) _(i) _(+k) _(j) =Σ_(k) _(λ) ^(f) ^(i) +Σ_(k) _(j) ^(f)^(j) and w_(k) _(i) _(,k) _(j) _(,t)=(2πσ_(k) _(λ) _(+k) _(j)^(t,τ))^(−1/2). Since z_(k) _(i) _(,k) _(j) _(,t,τ) ⁻¹ is the couplingbetween f_(t) ^(i) and f_(τ) ^(j), the value Π_(τ=1) ^(T)(1−z_(k) _(i)_(,k) _(j) _(,t,τ) ⁻¹) is the decoupling between agent i at t—f_(t)^(i)—and the trajectory of agent j, f^(j).

Definition 5. The symbol

(¬κ)—the probability of not colliding—represents the decoupling of

and

by

${{\mathbb{P}}\left( {\neg\kappa} \right)} = {\prod\limits_{r = 1}^{T}{\prod\limits_{\tau = 1}^{T}\left( {1 - Z_{k_{i},k_{j},t,\tau}^{- 1}} \right)}}$

Definition 6. The transform P¬κ measures how decoupled the host andagent GPs

_(f) _(η) _(,k) _(i) and

_(fjk) _(j) are:P _(¬κ):

_(f) _(i) _(,k) _(i) →Π_(t=1) ^(T)Π_(τ=1) ^(T)(1−z _(k) _(i) _(,k) _(j)_(,t,τ) ⁻¹)

_(f) _(i) _(,k) _(i)

_(fjk)=∧^(k) ^(?) ^(,k) ^(j)

_(f) _(i) _(,k) _(t)

_(fjk) _(j) .where ∧^(k) ^(i) ^(,k) ^(j) ≡Π_(t=1) ^(T)Π_(τ=1) ^(T)(1−z _(k) _(i)_(,k) _(j) _(,t,τ) ⁻¹).

Agent models may be defined as f_(t) ^(i)=h(f_(t−1) ^(i), η_(t)), η_(t)˜

(0, σ_(η) _(t) ). This induces a diagonal trajectory covariance matrix:agent flexibility at t is decoupled from flexibility at t+1.Accordingly, the objective function is applied to the set of Gaussianprocesses for the host 302 and the agents 304-312. The objectivefunction may be applied to a Gaussian process by:

$\begin{matrix}{{p\left( {f^{R},{f^{i}{❘z_{1:t}}}} \right)} = {{P_{\neg\kappa}\left\lbrack {\sum\limits_{\ell = 1}^{N_{t}^{R}}{w_{\ell}^{R}\mathcal{N}_{f^{R},\ell}{\sum\limits_{k_{i} = 1}^{N_{t}^{f^{i}}}{w_{k_{i}}^{f^{i}}\mathcal{N}_{f^{i},k_{i}}}}}} \right\rbrack} = {\sum\limits_{\ell = 1}^{N_{t}^{R}}{\sum\limits_{k_{i} = 1}^{N_{t}^{f^{i}}}{\Lambda_{l,}k_{i}w_{\ell}^{R}w_{k_{i}}^{f^{i}}\mathcal{N}_{f^{R},\ell}\mathcal{N}_{f^{\overset{.}{x}},k_{i}}}}}}} & \left( {{Eq}.0.7} \right)\end{matrix}$

Eq. 0.7 can be generalize IGP to n_(t)≥1, such that for a multi-agentsecond order Gaussian process can be given by

${p\left( {f^{R},{f{❘z_{1:t}}}} \right)} = {{p_{\neg\kappa}^{IGP}\left\lbrack {\overset{N_{t}^{R}}{\sum\limits_{k_{R} = 1}}{W_{k_{R}}^{R}\mathcal{N}_{f^{R},k_{R}}{\underset{i = 1}{\prod\limits^{n_{t}}}{\overset{N_{t}^{f^{i}}}{\sum\limits_{k_{i} = 1}}{w_{k_{i}}^{f^{i}}\mathcal{N}_{f^{i},k_{i}}}}}}} \right\rbrack} = {p_{\neg\kappa}^{IGP}\left\lbrack {{\overset{N_{t}^{R}}{\sum\limits_{k_{R} = 1}}{w_{k_{R}}^{R}\mathcal{N}_{f^{R},k_{B}} \times {\overset{N_{t}^{f^{i}}}{\sum\limits_{k_{i} = 1}}{w_{k_{1}}^{f^{1}}N_{f^{1},k_{1}} \times \ldots \times {\overset{N_{t}^{f^{n}t}}{\sum\limits_{k_{n_{t}} = 1}}{w_{k_{n_{t}}}^{f^{{r\iota},}}\mathcal{N}_{f^{\mathfrak{n}},,k_{n_{t}}}}}}}}} = {\overset{N_{BTG}}{\sum\limits_{\eta = 1}}{\left\lbrack {w\Lambda} \right\rbrack_{\eta}\left\lbrack {\mathcal{N}_{f^{R}}\mathcal{N}_{f^{1}}\ldots\mathcal{N}_{f^{n_{t}}}} \right\rbrack}_{\eta}}} \right.}}$

where N_(BIG)=N_(t) ^(R)Π_(i=1) ^(n) ^(t) N_(t) ^(f) ^(i) and ηenumerates all products of robot and agent GPs. If 1>R, then thecoefficients pa [wΛ]_(η)=Π_(i=R) ^(n)w_(η) ^(f) ^(t) Π_(j>i) ^(n)Λ_(η)^(k) ^(i) ^(,k) ^(j) weight each GP basis element [

_(f) _(R)

_(f) ₁ . . .

_(f) _(n) _(t)]_(η)=

_(f) _(R) _(,η)

_(f) ₁ _(,η) . . .

_(f) _(n) _(,η) according to p_(¬κ) ^(IGP). The operator p_(¬κ) ^(IGP)operates pairwise p_(¬κ) ^(IGP) ^(≡) (p_(¬κ))^(Σ) ^(m=0) ^(n) ^(t) ⁻¹^((n) ^(t) ^(−m))=p_(¬κ) _(oP) _(¬Λ) _(o . . . oP) _(¬κ), on host-agentpairs and as well as agent-pairs. Accordingly, the objective functionfor modeling first order interactions and second order interactions maybe given by.

$\left( {f^{R},{f{❘z_{1:t}}}} \right) = {{p_{\neg\kappa}^{IGP}\left\lbrack {\overset{N_{t}^{R}}{\sum\limits_{k_{R} = 1}}{W_{k_{R}}^{R}\mathcal{N}_{f^{R},k_{R}}{\underset{i = 1}{\prod\limits^{n_{t}}}{\overset{N_{t}^{f^{i}}}{\sum\limits_{k_{i} = 1}}{w_{k_{i}}^{f^{i}}\mathcal{N}_{f^{i},k_{i}}}}}}} \right\rbrack} = {p_{\neg\kappa}^{IGP}\left\lbrack {{\overset{N_{t}^{R}}{\sum\limits_{k_{R} = 1}}{w_{k_{R}}^{R}\mathcal{N}_{f^{R},k_{B}} \times {\sum\limits_{k_{1} = 1}^{N_{t}^{f^{1}}}{w_{k_{1}}^{f^{1}}N_{f^{1},k_{1}} \times \ldots \times {\sum\limits_{k_{n_{t}} = 1}^{N_{t}^{f^{n}t}}{w_{k_{n_{t}}}^{f^{{r\iota},}}\mathcal{N}_{f^{\mathfrak{n}},,k_{n_{t}}}}}}}}} = {\overset{N_{BTG}}{\sum\limits_{\eta = 1}}{\left\lbrack {w\Lambda} \right\rbrack_{\eta}\left\lbrack {\mathcal{N}_{f^{R}}\mathcal{N}_{f^{1}}\ldots\mathcal{N}_{f^{n_{t}}}} \right\rbrack}_{\eta}}} \right.}}$

where N_(BIG)=N_(t) ^(R)Π_(i=1) ^(n) ^(t) N_(t) ^(f) ^(i) and ηenumerates all products of host and agent Gaussian processes. Thecoefficients [Λw]_(η)=Λ_(η) ^(R,f) ¹ . . . Λ_(η) ^(R,f) ^(n) ^(t)w_(η)^(R)w_(η) ^(f) ¹ . . . w_(η) ^(f) ^(n) ^(t) weight each Gaussian processbasis element [

_(f) _(R)

_(f) ₁ . . .

_(f) _(n) _(t)]_(η)=

_(f) _(R) _(,η)

_(f) ₁ _(,η) . . .

_(f) _(n) _(t,η) according to p_(¬κ) ^(IGP). The operator P_(¬κ) ^(IGP)operates pairwise P_(¬κ) ^(IGP)≡(P_(¬κ) ^(IGP))^(n) ^(t) =P_(¬κ)∘P_(¬κ)∘. . . ∘P_(¬κ) such that the agents 304-312 can be measured with respectto the host 302.

Instead of brute force enumeration, the N*«N_(BIG) modes are determinedthat capture Equation 0.8, in a process called optimal shaping such thata convex configuration of the number of agent in the physicalenvironment is determined. Accordingly, the modes with the most likelyprobability are determined.

Firstly, μ_(l) ^(R), μ_(k) _(i) ^(f) ^(i) are treated as functions

, x_(f) _(i) _(,k) _(i) ∈

(

)→

² mapping time to (x,y) position and search for the

, x_(f) _(i) _(,k) _(i) * that optimize [Λw]_(η) such that:

$w_{x_{R,\ell}} = {\mathcal{N}\left( {x_{R,\ell}{❘{\mu_{\ell}^{R},{{\overset{R}{\underset{\ell)}{\Sigma}} w_{x_{f^{i},k_{i}}}} = {\mathcal{N}\left( {x_{f^{i},k_{i}}{❘{\mu_{k_{i}}^{f^{i}},{{\overset{f^{i}}{\underset{k_{i})}{\Sigma}} Z_{x_{R,\ell,t,}x_{f^{i},k_{i},\tau}}^{- 1}} = {{{\exp\left\lbrack {{- \frac{1}{2}}\left( {{x_{R,\ell,t} - x_{f^{i}}},{ki},_{T}} \right){T\left( {\sigma_{\ell + k_{i}}^{t,\tau}{\mathbb{I}\mathbb{I}}} \right)}^{- 1}\left( {x_{R,\ell,t} - x_{{f^{i}k_{i}},\tau}} \right)} \right\rbrack}{where}X_{R,\ell,t}} = {x_{R,\ell}(t)}}},{x_{f^{i},k_{i},\tau} = {{x_{f^{i},k_{i}}(\tau)} \in {{\mathbb{R}}^{2}.}}}}}} \right.}}}}} \right.}$

If

x_(f) = [x_(f¹, k₁,)x_(f^(n)t, k_(n_(t)))],then an objective function of the interacting function may be given by

${\lambda_{n_{t}}\left( {x_{R,l},x_{f}} \right)} \equiv {w_{x_{R,\ell}}\overset{n_{t}}{\underset{i = 1}{\Pi}}\underset{t = 1}{\overset{T}{\Pi}}{\underset{\tau = 1}{\overset{T}{\Pi}}\left( {1 - Z_{x_{R,\ell,t,}x_{f^{i},k_{i},\tau}}^{- 1}} \right)}w_{x_{f^{i},k_{i}}}}$

A logarithm can be used to reduce the optimization computational burdenand increase numerical accuracy such that:

$\begin{matrix}{{\log{\lambda_{n_{t}}\left( {x_{R,l},x_{f}} \right)}} = {{\sum\limits_{i = 1}^{n_{t}}{\sum\limits_{t = 1}^{T}{\sum\limits_{\tau = 1}^{T}{\log\left( {1 - Z_{x_{R,\ell,t,}x_{f^{i},k_{i},\tau}}^{- 1}} \right)}}}} - {\frac{1}{2}\left( {x_{R,l} - \mu_{\ell}^{R}} \right)^{T}\left( \overset{R}{\Sigma} \right)^{- 1}\left( {x_{R,l} - \mu_{\ell}^{R}} \right)} - {\overset{n_{t}}{\underset{i = 1}{\Sigma}}\frac{1}{2}\left( {x_{f^{\overset{.}{t}},k_{i}} - \mu_{k_{i}}^{f^{i}}} \right)^{T}\left( \overset{f^{i}}{\Sigma} \right)^{- 1}\left( {x_{f^{i},k_{2}}.{- \mu_{k_{i}}^{f^{i}}}} \right)}}} & \left( {{Eq}.0.9} \right)\end{matrix}$

So that the convex configuration of the agents 304-312 in the physicalenvironment is given by:[

,x _(f)*]=arg, max log [λ_(n) _(t) (x _(R,l) ,x _(f))]  (Eq. 0.10)

The objective function defines the intent. Returning to the examplegiven above, suppose the intent of the host 302 is to avoid a collisionwith the agent 312 while also continuing to move in as direct a path aspossible. Avoiding a collision and encouraging cooperation may be givenby:

${\alpha_{n_{t}}\left( {x_{R,\ell},x_{f}} \right)} = {\sum\limits_{i = 1}^{n_{t}}{\prod\limits_{t = 1}^{T}{\prod\limits_{\tau = 1}^{T}{\log\left( {1 - Z_{x_{R,\ell,t,}x_{f^{i},k_{i},\tau}}^{- 1}} \right)}}}}$

while continuing to move in as direct a path, such as a straight linemay be given by:β_(n) _(t) (x _(R,l) ,x _(f))=−½(x _(R,l)−

)^(T)(

)⁻¹(

−

)=−½Σ_(i=1) ^(n) ^(t) (x _(f) _(i) _(,k) _(i) n _(t)−μ_(k) _(i) ^(f)^(i) )^(T)(Σ_(k) _(i) ^(f) ^(i) )⁻¹(x _(f) _(i) _(,k) _(i) −μ_(k) _(i)^(f) ^(i) ).

Accordingly, while α_(n) _(t) (x_(R,l), x_(f)) encourages a cooperativeprotocol and prevents collisions, β_(n) _(t) (x_(R,l), x_(f)) penalizesactions that deviate from agent intent. Unless the optimal values forβ_(n) _(t) (x_(R,l), x_(f)) already optimize λ_(n) _(t) (x_(R,l),x_(f)), α_(n) _(t) (x_(R,l), x_(f)) moves x_(R,l), x_(f) away fromintent.

To generate the convex configuration, the optimizations are seeded with(μ^(R), μ^(f))±(0, [σ^(R), σ^(f)], 2[σ^(R), σ^(f)], 3[σ^(R), σ^(f)]),where σ^(R)=√{square root over (diag(Σ^(R)))}, σ^(f)=√{square root over(diag(Σ^(f)))}. The optimizations provide insight about the agents304-312. In particular, the optimizations may be calculated for eachagent of the agents 304-312 and the host 302 and then computed theeffective sample size of n_(t) host-agent pairs to determine how manyagents are statistically significant to the optimization. In thismanner, a subset of the agents 304-312 can be determined for each timestep.

Once the convex configuration is determined it may be used by the brakesystem 124, the path planning module 126, the notification system 128,and the sensor system 130 to alter the functioning of the host 302, theagents 304-312, and/or infrastructure, such as the sensor module 102.For example, the brake system 124 may transmit an instruction for thehost 302 and/or the agents 304-312 to brake based on the convexconfiguration. The path planning module 126 may plan a path or adjust apath of the host 302 based on the convex configuration modeled by crowd.

In another embodiment, the notification system 128 may notify the agents304-312 based on the convex configuration, for example, the agents304-312 may notified of the path of the host 302. In another embodiment,the sensor system 130 may adjust the manner and/or location that issensed within the physical environment.

In this manner, Gaussian Processes can be used to model the jointinteraction between the host 302 and the agents 304-312 as well as theagents 304-312 among each other. The Gaussian Processes could model aninfinite number of trajectories, however most trajectories are notlikely. For example, it is not likely that an agent will zig-zag to atarget. Instead, the trajectories are grouped into lanes and optimizeover the function space and a Gaussian Processes is calculated for eachagent based on the host-agent pairs as well as the agent-agent pairs.The Gaussian processes are multiplied by an objective function having amean indicative of intent and a variance indicative of flexibility suchthat the Gauss. Thus, the means become the basis set for a distributionof the Gaussian Processes. In this manner, the intent can be used toidentify high value lanes. Further we allow the constants to vary basedon the flexibility.

Still another aspect involves a computer-readable medium includingprocessor-executable instructions configured to implement one aspect ofthe techniques presented herein. An aspect of a computer-readable mediumor a computer-readable device devised in these ways is illustrated inFIG. 4 , wherein an implementation 400 includes a computer-readablemedium 408, such as a CD-R, DVD-R, flash drive, a platter of a hard diskdrive, etc., on which is encoded computer-readable data 406. Thisencoded computer-readable data 406, such as binary data including aplurality of zero's and one's as shown in 406, in turn includes a set ofprocessor-executable computer instructions 404 configured to operateaccording to one or more of the principles set forth herein. In thisimplementation 400, the processor-executable computer instructions 404may be configured to perform a method 402, such as the method 200 ofFIG. 2 . In another aspect, the processor-executable computerinstructions 404 may be configured to implement a system, such as theoperating environment 100 of FIG. 1 . Many such computer-readable mediamay be devised by those of ordinary skill in the art that are configuredto operate in accordance with the techniques presented herein.

As used in this application, the terms “component”, “module,” “system”,“interface”, and the like are generally intended to refer to acomputer-related entity, either hardware, a combination of hardware andsoftware, software, or software in execution. For example, a componentmay be, but is not limited to being, a process running on a processor, aprocessing unit, an object, an executable, a thread of execution, aprogram, or a computer. By way of illustration, both an applicationrunning on a controller and the controller may be a component. One ormore components residing within a process or thread of execution and acomponent may be localized on one computer or distributed between two ormore computers.

Further, the claimed subject matter is implemented as a method,apparatus, or article of manufacture using standard programming orengineering techniques to produce software, firmware, hardware, or anycombination thereof to control a computer to implement the disclosedsubject matter. The term “article of manufacture” as used herein isintended to encompass a computer program accessible from anycomputer-readable device, carrier, or media. Of course, manymodifications may be made to this configuration without departing fromthe scope or spirit of the claimed subject matter.

FIG. 5 and the following discussion provide a description of a suitablecomputing environment to implement aspects of one or more of theprovisions set forth herein. The operating environment of FIG. 5 ismerely one example of a suitable operating environment and is notintended to suggest any limitation as to the scope of use orfunctionality of the operating environment. Example computing devicesinclude, but are not limited to, personal computers, server computers,hand-held or laptop devices, mobile devices, such as mobile phones,Personal Digital Assistants (PDAs), media players, and the like,multiprocessor systems, consumer electronics, mini computers, mainframecomputers, distributed computing environments that include any of theabove systems or devices, etc.

Generally, aspects are described in the general context of “computerreadable instructions” being executed by one or more computing devices.Computer readable instructions may be distributed via computer readablemedia as will be discussed below. Computer readable instructions may beimplemented as program modules, such as functions, objects, ApplicationProgramming Interfaces (APIs), data structures, and the like, thatperform one or more tasks or implement one or more abstract data types.Typically, the functionality of the computer readable instructions arecombined or distributed as desired in various environments.

FIG. 5 illustrates a system 500 including an apparatus 512 configured toimplement one aspect provided herein. In one configuration, theapparatus 512 includes at least one processing unit 516 and memory 518.Depending on the exact configuration and type of computing device,memory 518 may be volatile, such as RAM, non-volatile, such as ROM,flash memory, etc., or a combination of the two. This configuration isillustrated in FIG. 5 by dashed line 514.

In other aspects, the apparatus 512 includes additional features orfunctionality. For example, the apparatus 512 may include additionalstorage such as removable storage or non-removable storage, including,but not limited to, magnetic storage, optical storage, etc. Suchadditional storage is illustrated in FIG. 5 by storage 520. In oneaspect, computer readable instructions to implement one aspect providedherein are in storage 520. Storage 520 may store other computer readableinstructions to implement an operating system, an application program,etc. Computer readable instructions may be loaded in memory 518 forexecution by processing unit 516, for example.

The term “computer readable media” as used herein includes computerstorage media. Computer storage media includes volatile and nonvolatile,removable and non-removable media implemented in any method ortechnology for storage of information such as computer readableinstructions or other data. Memory 518 and storage 520 are examples ofcomputer storage media. Computer storage media includes, but is notlimited to, RAM, ROM, EEPROM, flash memory or other memory technology,CD-ROM, Digital Versatile Disks (DVDs) or other optical storage,magnetic cassettes, magnetic tape, magnetic disk storage or othermagnetic storage devices, or any other medium which may be used to storethe desired information and which may be accessed by the apparatus 512.Any such computer storage media is part of the apparatus 512.

The term “computer readable media” includes communication media.Communication media typically embodies computer readable instructions orother data in a “modulated data signal” such as a carrier wave or othertransport mechanism and includes any information delivery media. Theterm “modulated data signal” includes a signal that has one or more ofits characteristics set or changed in such a manner as to encodeinformation in the signal.

The apparatus 512 includes input device(s) 524 such as keyboard, mouse,pen, voice input device, touch input device, infrared cameras, videoinput devices, or any other input device. Output device(s) 522 such asone or more displays, speakers, printers, or any other output device maybe included with the apparatus 512. Input device(s) 524 and outputdevice(s) 522 may be connected to the apparatus 512 via a wiredconnection, wireless connection, or any combination thereof. In oneaspect, an input device or an output device from another computingdevice may be used as input device(s) 524 or output device(s) 522 forthe apparatus 512. The apparatus 512 may include communicationconnection(s) 526 to facilitate communications with one or more otherdevices 530, such as through network 528, for example.

Although the subject matter has been described in language specific tostructural features or methodological acts, it is to be understood thatthe subject matter of the appended claims is not necessarily limited tothe specific features or acts described above. Rather, the specificfeatures and acts described above are disclosed as example aspects.Various operations of aspects are provided herein. The order in whichone or more or all of the operations are described should not beconstrued as to imply that these operations are necessarily orderdependent. Alternative ordering will be appreciated based on thisdescription. Further, not all operations may necessarily be present ineach aspect provided herein.

As used in this application, “or” is intended to mean an inclusive “or”rather than an exclusive “or”. Further, an inclusive “or” may includeany combination thereof (e.g., A, B, or any combination thereof). Inaddition, “a” and “an” as used in this application are generallyconstrued to mean “one or more” unless specified otherwise or clear fromcontext to be directed to a singular form. Additionally, at least one ofA and B and/or the like generally means A or B or both A and B. Further,to the extent that “includes”, “having”, “has”, “with”, or variantsthereof are used in either the detailed description or the claims, suchterms are intended to be inclusive in a manner similar to the term“comprising”.

Further, unless specified otherwise, “first”, “second”, or the like arenot intended to imply a temporal aspect, a spatial aspect, an ordering,etc. Rather, such terms are merely used as identifiers, names, etc. forfeatures, elements, items, etc. For example, a first channel and asecond channel generally correspond to channel A and channel B or twodifferent or two identical channels or the same channel. Additionally,“comprising”, “comprises”, “including”, “includes”, or the likegenerally means comprising or including, but not limited to.

It will be appreciated that several of the above-disclosed and otherfeatures and functions, or alternatives or varieties thereof, may bedesirably combined into many other different systems or applications.Also that various presently unforeseen or unanticipated alternatives,modifications, variations or improvements therein may be subsequentlymade by those skilled in the art which are also intended to beencompassed by the following claims.

The invention claimed is:
 1. A system for crowd navigation of a hostvehicle among a plurality of agents, comprising: a processor configuredto receive sensor data; a statistical module, implemented via theprocessor, configured to: identify a number of agents in a physicalenvironment based on the sensor data; calculate a set of Gaussianprocesses, wherein the set of Gaussian processes includes a GaussianProcess for each agent of the number of agents; and determine anobjective function based on an intent and a flexibility for the host andat least two agents of the plurality of agents to classify first orderinteractions and second order interactions, wherein the first orderinteractions include interactions between the host and one of the atleast two agents, and wherein the second order interactions includeinteractions between agents of the at least two agents; and a modelmodule, implemented via the processor, configured to: generate a modelof the number of agents by applying the objective function to the set ofGaussian processes, wherein the model includes a convex configuration ofthe physical environment.
 2. The system of claim 1, wherein the sensordata is received from a sensor system of the host.
 3. The system ofclaim 1, wherein the intent is based on a goal of the host, and whereinthe flexibility is based on a willingness of the host to deviate fromthe goal.
 4. The system of claim 1, wherein the intent is a mean of theobjective function and the flexibility is a covariance of the objectivefunction.
 5. The system of claim 1, wherein the model module is furtherconfigured to perform optimal shaping to determine a subset of agents ofthe number of agents that are statistically significant, and wherein theconvex configuration is based on the subset of agents.
 6. The system ofclaim 1, wherein the host is a robot, and wherein agents of the numberof agents are humans.
 7. A method for crowd navigation of a host vehicleamong a plurality of agents, comprising: identifying a number of agentsin a physical environment based on sensor data from a host; calculatinga set of Gaussian processes, wherein the set of Gaussian processesincludes a Gaussian Process for each agent of the number of agents;determining an objective function based on an intent and a flexibilityfor the host and at least two agents of the plurality of agents toclassify first order interactions and second order interactions, whereinthe first order interactions include interactions between the host andone of the at least two agents, and wherein the second orderinteractions include interactions between agents of the at least twoagents; and generating a model of the number of agents by applying theobjective function to the set of Gaussian processes, wherein the modelincludes a convex configuration of the physical environment.
 8. Themethod of claim 7, wherein the sensor data is received from a sensorsystem of the host.
 9. The method of claim 7, wherein the intent isbased on a goal of the host, and wherein the flexibility is based on awillingness of the host to deviate from the goal.
 10. The method ofclaim 7, wherein the intent is a mean of the objective function and theflexibility is a covariance of the objective function.
 11. The method ofclaim 7, further comprising: performing optimal shaping to determine asubset of agents of the number of agents that are statisticallysignificant, and wherein the convex configuration is based on the subsetof agents.
 12. The method of claim 7, wherein the host is a robot, andwherein agents of the number of agents are humans.
 13. The method ofclaim 7, wherein the intent is based on collision avoidance of the host,and the flexibility is based on willingness of the host to deviate froma direct path.
 14. A non-transitory computer readable storage mediumstoring instructions that when executed by a computer having a processorto perform a method for crowd navigation of a host vehicle among aplurality of agents, the method comprising: identifying a number ofagents in a physical environment based on sensor data received from ahost; calculating a set of Gaussian processes, wherein the set ofGaussian processes includes a Gaussian Process for each agent of thenumber of agents; determining an objective function based on an intentand a flexibility for the host and at least two agents of the pluralityof agents to classify first order interactions and second orderinteractions, wherein the first order interactions include interactionsbetween the host and one of the at least two agents, and wherein thesecond order interactions include interactions between agents of the atleast two agents; and generating a model of the number of agents byapplying the objective function to the set of Gaussian processes,wherein the model includes a convex configuration of the physicalenvironment.
 15. The non-transitory computer readable storage medium ofclaim 14, wherein the sensor data is received from a sensor system ofthe host.
 16. The non-transitory computer readable storage medium ofclaim 14, wherein the intent is based on a goal of the host, and whereinthe flexibility is based on a willingness of the host to deviate fromthe goal.
 17. The non-transitory computer readable storage medium ofclaim 14, wherein the intent is a mean of the objective function and theflexibility is a covariance of the objective function.
 18. Thenon-transitory computer readable storage medium of claim 14, furthercomprising: performing optimal shaping to determine a subset of agentsof the number of agents that are statistically significant, and whereinthe convex configuration is based on the subset of agents.
 19. Thenon-transitory computer readable storage medium of claim 14, wherein thehost is a robot, and wherein agents of the number of agents are humans.20. The non-transitory computer readable storage medium of claim 14,wherein the intent is based on collision avoidance of the host, and theflexibility is based on willingness of the host to deviate from a directpath.